#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "tinyusb.h"
#include "class/hid/hid_device.h"
#include "espFFB.h"
#include "USBdesc.h"
#include "MathHelper.h"
#include "port.h"
#include "esp_heap_caps.h"
// #include "driver/timer.h"

#include "Motor.h"
#include "driver/timer.h"
#include "ParameterManager.h"

char *TAG = "ESP_FFB";
uint8_t DeviceGain = 0;

static RamPool _ramPool;

PlayingManager EffectPlayingManager;

PidBlockLoadReport _pidBlockLoadReport;

WorldModel worldModel = {
    .deviceState = {
        .effect_state = 0x00,
        .pid_state = 0x00
    }
};


// Condition Conditions[MAX_EFFECTS_NUM] = {};
// SetPeriodicReport PeriodicForces[MAX_EFFECTS_NUM] = {};
//SetConstantForceReport ConstantForces[MAX_EFFECTS_NUM] = {};
//SetCustomForceReport CustomForces[MAX_EFFECTS_NUM] = {};
SetEnvelopeReport EnvelopeReports[MAX_EFFECTS_NUM] = {};
//SetRampForceReport RampReport[MAX_EFFECTS_NUM] = {};

/**
 * @brief Get the Next Index of Ram Pool,but index start in 1 
 * 
 * @return the min index which can be used (index of array plus 1),0 means pool full
 */
uint8_t GetNextIndex(void){
    for(int i = 0;i<MAX_EFFECTS_NUM;i++){
        if (_ramPool.ForcesPool[i].State == MEFFECTSTATE_FREE){
            return i+1;
        }
    }
    return 0;
}
void InitRamPool(void){
    _ramPool.UsedCount = 0;
    for(int i = 0;i<MAX_EFFECTS_NUM;i++){
        _ramPool.ForcesPool[i].State = MEFFECTSTATE_FREE;
        _ramPool.ForcesPool[i].EnveLpoePara = NULL;

        // _RamPool.ForcesPool[i].ConstantPara = NULL;
        memset(_ramPool.ForcesPool[i].EffectPara.array_entry,0,sizeof(_ramPool.ForcesPool[i].EffectPara.array_entry));
        //memset(_RamPool.ForcesPool[i].ConditionPara.array_entry,0,sizeof(_RamPool.ForcesPool[i].EffectPara.array_entry));
        //_RamPool.ForcesPool[i].EffectPara.array_entry
    }
}
void FreeBlock(uint8_t block_index){
    if(_ramPool.ForcesPool[block_index-1].State != MEFFECTSTATE_FREE){
        _ramPool.ForcesPool[block_index-1].State = MEFFECTSTATE_FREE;
        _ramPool.ForcesPool[block_index-1].EnveLpoePara = NULL;
        _ramPool.UsedCount--;
        //printf("Free Block %d count %d\n",block_index,_ramPool.UsedCount);
    }   
}
void FreeAllBlocks(void){
    for(int i = 1;i<=MAX_EFFECTS_NUM;i++){
        FreeBlock(i);
    }
}
/************************************* Effect API**************************************/
/**
 * @brief
 *  
 * @param
 */

void CreateNewEffect(uint8_t reportID,const uint8_t *hid_report_out,uint8_t length)
{   

    // block_index �� �ڴ�������Ĺ�ϵ�� �ڴ������ = block_index -1
    uint8_t _blockIndex = GetNextIndex();
    _pidBlockLoadReport.effect_block_index = _blockIndex;
    if(_blockIndex!=0){ //�������ʣ��ռ�
        _pidBlockLoadReport.block_load_status = SUCCESS;
        _pidBlockLoadReport.ram_pool_available = (MAX_EFFECTS_NUM - _ramPool.UsedCount)*sizeof(Force_t); 
        _ramPool.ForcesPool[_blockIndex-1].State =  MEFFECTSTATE_ALLOCATED;
        _ramPool.UsedCount++;
        //printf("Create New Effect Type %d, Block Index %d\n",hid_report_out[0],_blockIndex);
    }
    else{
        _pidBlockLoadReport.block_load_status = OUT_OF_MEMORY;
        _pidBlockLoadReport.ram_pool_available = 0x00; 
    }
    
 }
/**
 * @brief ����block����
 * 
 * @return uint8_t
 * 
 * @todo δ��� 
 */
static void SetEffect(const uint8_t* buffer,uint16_t bufsize){
    uint8_t _blockIndex = buffer[1];
    uint8_t _lastState = EffectPlayingManager.PlayList[_blockIndex-1].PlayState;
    EffectPlayingManager.PlayList[_blockIndex-1].PlayState = STOPPED;
    memcpy(&(_ramPool.ForcesPool[_blockIndex-1].EffectPara.array_entry),buffer,bufsize);
    EffectPlayingManager.PlayList[_blockIndex-1].PlayState = _lastState;
//     printf("Set Effect Block Index %u \n",_ramPool.ForcesPool[_blockIndex-1].EffectPara.set_effect_report.effect_block_index);
//     printf("Report ID: %u\n", _ramPool.ForcesPool[_blockIndex-1].EffectPara.set_effect_report.report_id);
//     //printf("Effect Block Index: %d\n", _ramPool.ForcesPool[_blockIndex-1].EffectPara.set_effect_report.effect_block_index);
//     printf("Effect Type: %u\n", _ramPool.ForcesPool[_blockIndex-1].EffectPara.set_effect_report.effect_type);
//     printf("Duration: %d ms\n", _ramPool.ForcesPool[_blockIndex-1].EffectPara.set_effect_report.duration);
//     printf("Trigger Repeat Interval: %d ms\n", _ramPool.ForcesPool[_blockIndex-1].EffectPara.set_effect_report.trigger_repeat_interval);
//     printf("Sample Period: %d ms\n", _ramPool.ForcesPool[_blockIndex-1].EffectPara.set_effect_report.sample_period);
//     printf("Gain: %d\n", _ramPool.ForcesPool[_blockIndex-1].EffectPara.set_effect_report.gain);
//     printf("Trigger Button: %d\n", _ramPool.ForcesPool[_blockIndex-1].EffectPara.set_effect_report.trigger_button);
//     printf("Axes Enable Direction: %d\n", _ramPool.ForcesPool[_blockIndex-1].EffectPara.set_effect_report.axes_enable_direction);
//     printf("Direction with Ordinals 1 (X axis): %d\n", _ramPool.ForcesPool[_blockIndex-1].EffectPara.set_effect_report.direction_with_ordinals_1);
//     printf("Direction with Ordinals 2 (Y axis): %d\n", _ramPool.ForcesPool[_blockIndex-1].EffectPara.set_effect_report.direction_with_ordinals_2);
//     printf("Usage Undefined 1: %d\n", _ramPool.ForcesPool[_blockIndex-1].EffectPara.set_effect_report.usage_undefined_1);
}   
static void SetEnvelope(const uint8_t* buffer,uint16_t bufsize){
    uint8_t _blockIndex = buffer[1];
    memcpy((EnvelopeReports[_blockIndex-1].array_entry),buffer,bufsize);
    _ramPool.ForcesPool[_blockIndex-1].EnveLpoePara = &(EnvelopeReports[_blockIndex-1]);
    //printf("Set Envelope Block Index %d, \n",_ramPool.ForcesPool->EnveLpoePara->EnvelopeReport.effectBlockIndex);
    // printf("attackLevel %d, fadeLevel %d, attackTime %d, fadeTime %d \n",
    //     _ramPool.ForcesPool[_blockIndex-1].EnveLpoePara->EnvelopeReport.attackLevel,
    //     _ramPool.ForcesPool[_blockIndex-1].EnveLpoePara->EnvelopeReport.fadeLevel,
    //     _ramPool.ForcesPool[_blockIndex-1].EnveLpoePara->EnvelopeReport.attackTime,
    //     _ramPool.ForcesPool[_blockIndex-1].EnveLpoePara->EnvelopeReport.fadeTime
    // );
}
static void SetPeriodic(const uint8_t* buffer,uint16_t bufsize){
    uint8_t _blockIndex = buffer[1];
    memcpy(&(_ramPool.ForcesPool[_blockIndex-1].SpecialPara.Periodic),buffer,bufsize);
}

static void SetCondition(const uint8_t* buffer,uint16_t bufsize){
    uint8_t _blockIndex = buffer[1];
    if(buffer[2]==0){ //�ж���
        memcpy(_ramPool.ForcesPool[_blockIndex-1].SpecialPara.Condition[0].array_entry,buffer,bufsize);
        // printf("index %d\n",_ramPool.ForcesPool[_blockIndex-1].SpecialPara.Condition[0].set_condition_struct.usage_effect_block_index);
        // printf("cp_offset:%d\n",_ramPool.ForcesPool[_blockIndex-1].SpecialPara.Condition[0].set_condition_struct.cp_offset);
        // printf("positive_coefficient:%d\n",_ramPool.ForcesPool[_blockIndex-1].SpecialPara.Condition[0].set_condition_struct.positive_coefficient);
        // printf("negative_coefficient%d\n",_ramPool.ForcesPool[_blockIndex-1].SpecialPara.Condition[0].set_condition_struct.negative_coefficient);
        // printf("positive_saturation%d\n",_ramPool.ForcesPool[_blockIndex-1].SpecialPara.Condition[0].set_condition_struct.positive_saturation);
        // printf("negative_saturation%d\n",_ramPool.ForcesPool[_blockIndex-1].SpecialPara.Condition[0].set_condition_struct.negative_saturation);
        // printf("dead_band%d\n",_ramPool.ForcesPool[_blockIndex-1].SpecialPara.Condition[0].set_condition_struct.dead_band);
    }else if(buffer[2]==1){
        memcpy(_ramPool.ForcesPool[_blockIndex-1].SpecialPara.Condition[1].array_entry,buffer,bufsize);
    }
}
static void SetConstant(const uint8_t* buffer,uint16_t bufsize){
    uint8_t _blockIndex = buffer[1];
    memcpy(&(_ramPool.ForcesPool[_blockIndex-1].SpecialPara.Constant.array_entry),buffer,bufsize);
    //printf("Set Constant Block Index %d,mag %d \n",_ramPool.ForcesPool[_blockIndex-1].SpecialPara.Constant.set_constant_force_report.effect_block_index,_ramPool.ForcesPool[_blockIndex-1].SpecialPara.Constant.set_constant_force_report.magnitude);
}

static void SetRamp(const uint8_t* buffer,uint16_t bufsize){
    uint8_t _blockIndex = buffer[1];
    memcpy(&(_ramPool.ForcesPool[_blockIndex-1].SpecialPara.Ramp.array_entry),buffer,bufsize);
    //_RamPool.ForcesPool->RampPara = &RampReport[_blockIndex-1];
    //printf("Set Ramp Index %d, Start %d, End %d \n",
        //_ramPool.ForcesPool[_blockIndex-1].SpecialPara.Ramp.set_ramp_force_report.effect_block_index,
        //_ramPool.ForcesPool[_blockIndex-1].SpecialPara.Ramp.set_ramp_force_report.start,
        //_ramPool.ForcesPool[_blockIndex-1].SpecialPara.Ramp.set_ramp_force_report.end
        //);
}
static void ControlDevice(const uint8_t* buffer,uint16_t bufsize){
    uint8_t _blockIndex = buffer[1];
    switch (_blockIndex)
    {
        case DC_Enable_Actuators:
            SET_ACTUATOR_ENABLE(worldModel.deviceState);
            printf("Enable Actuators\n");
            break;
        case DC_Disable_Actuators:
            CLEAR_ACTUATOR_ENABLE(worldModel.deviceState);
            printf("Disable \n");
            break;
        case DC_Stop_All_Effects:
            printf("Stop All\n");
            break;
        case DC_Device_Reset:
            INIT_ALL_STATE(worldModel.deviceState);
            printf("Reset \n");
            break;
        case DC_Device_Pause:
            SET_PAUSE(worldModel.deviceState);
            printf("Pause \n");
            break;
        case DC_Device_Continue:
            CLEAR_PAUSE(worldModel.deviceState);
            printf("Continue \n");
            break;

        default:
            ESP_LOGI(TAG,"ERROR CONTRL");
            break;
        tud_hid_report(PID_STATE_REPORT,&worldModel.deviceState,sizeof(worldModel.deviceState));
    }
}

static void PlayEffect(uint8_t index,uint8_t loop){
    //printf("Start Play %d loop %d\n",index,loop);
    SET_BLOCK_STATE(worldModel.deviceState,index,true);
    Add2Play(&EffectPlayingManager,index,loop);
}
static void StopEffect(uint8_t index){
    //printf("Stop Play %d \n",index);
    SET_BLOCK_STATE(worldModel.deviceState,index,false);
    StopPlay(&EffectPlayingManager,index);
}
//Ч������
static void OperationEffect(const uint8_t* buffer,uint16_t bufsize){
    uint8_t _blockIndex = buffer[1];
    if(bufsize < 3){
        ESP_LOGI(TAG,"OperationEffect Bufsize Error!\n");
        return;
    }
    switch (buffer[2])
    {
        case 1:
            PlayEffect(_blockIndex,buffer[3]);
            //SET_BLOCK_STATE(deviceState,_blockIndex,true);
            break;
        case 2:
            
            // StartSolo
            // Stop all first
            // start given
            printf("Start Solo %d\n",_blockIndex);
            break;
        case 3:
            StopEffect(_blockIndex);
            //printf("Stop %d\n",_blockIndex);
            SET_BLOCK_STATE(worldModel.deviceState,_blockIndex,false);
            break;
        default:
            ESP_LOGI(TAG,"OperationEffect Para Error!\n");
            SET_BLOCK_STATE(worldModel.deviceState,_blockIndex,1);
            break;
    }
    //tud_hid_report(PID_STATE_REPORT,&worldModel.deviceState,sizeof(worldModel.deviceState));
    // memcpy(&(Conditions[_blockIndex-1].array_entry),buffer,bufsize);
    // _RamPool.ForcesPool->ConditionPara = &Conditions[_blockIndex-1];
    // //printf("Set Condition Block Index %d, \n",_RamPool.ForcesPool->ConditionPara->set_condition_struct.usage_effect_block_index);
}

void HandleOutput(const uint8_t* buffer,uint16_t bufsize){
    uint8_t out_report_id = buffer[0];
    //uint8_t _block_index = buffer[1]-1;
    ////printf("OUT %x\n",out_report_id);
    switch (out_report_id)
    {
        case SET_EFFECT_REPORT: 
            printf("SET_EFFECT_REPORT");
            SetEffect(buffer,bufsize);
            break;
        case SET_ENVELOPE_REPORT:
            SetEnvelope(buffer,bufsize);
            break;
        case SET_CONDITION_REPORT:
            SetCondition(buffer,bufsize);
            break;
        case SET_PERIODIC_REPORT:
            SetPeriodic(buffer,bufsize);
            break;
        case EFFICT_OPERATION_REPORT:
            OperationEffect(buffer,bufsize);
            break;
        case SET_CONSTANT_REPORT:
            SetConstant(buffer,bufsize);
            break;
        case SET_CUSTOM_FORCE_REPORT:
            break;
        case SET_RAMP_REPORT:
            SetRamp(buffer,bufsize);
            break;
        case DOWNLOAD_FORCE_SAMPLE:
            printf("DOWNLOAD_FORCE_SAMPLE X=%d,Y=%d\n",buffer[1],buffer[2]);
            break;
        case SET_CONTROL_DEVICE_REPORT:
            ControlDevice(buffer,bufsize);
        break;
        case SET_GAIN_REPORT:
            DeviceGain = buffer[1];
            //printf("DeviceGain %d\n",buffer[1]);
        break;
        case FREE_EFFECT_BLOCK_REPORT:
            FreeBlock(buffer[1]);
        break;
        default:
        break;
    }
}
void PlayingManagerInit(PlayingManager *PlayingManager){
}

// 这个代码在硬件定时器中断中被调用，如果在他被调用的时候发生了nvs操作，会报错导致重启，将这个代码放入ram中可解决
IRAM_ATTR void PlayingManagerUpdate(PlayingManager *PlayingManager){
    for(int i = 0;i<MAX_EFFECTS_NUM;i++){
        int16_t duration = _ramPool.ForcesPool[PlayingManager->PlayList[i].index-1].EffectPara.set_effect_report.duration;
        if(PlayingManager->PlayList[i].PlayState == PLAYING){
            PlayingManager->PlayList[i].Timoffset ++; //
        }
        if(duration!=INFINITE_DURATION && PlayingManager->PlayList[i].Timoffset > duration){
            PlayingManager->PlayList[i].PlayState = STOPPED; //
        }
    }
}
void Add2Play(PlayingManager *PlayingManager,uint8_t BlockIndex,uint8_t loop){
    PlayingManager->PlayList[BlockIndex-1].index = BlockIndex;
    PlayingManager->PlayList[BlockIndex-1].Playnum = loop;
    PlayingManager->PlayList[BlockIndex-1].PlayState = PLAYING;
    PlayingManager->PlayList[BlockIndex-1].Timoffset = 0;
    
}
void StopPlay(PlayingManager *PlayingManager,uint8_t BlockIndex){
    PlayingManager->PlayList[BlockIndex-1].PlayState = STOPPED;
    PlayingManager->PlayList[BlockIndex-1].Timoffset = 0;
}
void PausePlay(PlayingManager *PlayingManager,uint8_t BlockIndex){
    PlayingManager->PlayList[BlockIndex-1].PlayState = PAUSED;
}
void StartPlay(PlayingManager *PlayingManager,uint8_t BlockIndex){
    PlayingManager->PlayList[BlockIndex-1].PlayState = PLAYING;
}
int32_t ApplyGain(int16_t value, uint8_t gain)
{
	int32_t value_32 = value;
	return ((value_32 * gain) / 255);
}
int32_t ApplyEnvelope(Playing_t *PlayingEffect,int32_t value,uint8_t magnitude){ 
    //const uint8_t gain = _RamPool.ForcesPool[PlayingEffect->index-1].EffectPara.set_effect_report.gain;
    // // if(_RamPool.ForcesPool[PlayingEffect->index-1].ConstantPara!=NULL){
    //     magnitude = ApplyGain(_RamPool.ForcesPool[PlayingEffect->index-1].ConstantPara->set_constant_force_report.magnitude, DeviceGain);
    // // }
    int32_t newValue = value;
    int32_t duration = _ramPool.ForcesPool[PlayingEffect->index-1].EffectPara.set_effect_report.duration;
    if(duration == INFINITE_DURATION){
        return newValue;
    }
    else if(_ramPool.ForcesPool[PlayingEffect->index-1].EnveLpoePara!=NULL){
        int32_t attackLevel = ApplyGain(_ramPool.ForcesPool[PlayingEffect->index-1].EnveLpoePara->EnvelopeReport.attackLevel, DeviceGain);
        int32_t fadeLevel = ApplyGain(_ramPool.ForcesPool[PlayingEffect->index-1].EnveLpoePara->EnvelopeReport.fadeLevel, DeviceGain);

        int32_t attackTime = _ramPool.ForcesPool[PlayingEffect->index-1].EnveLpoePara->EnvelopeReport.attackTime;
        int32_t fadeTime = _ramPool.ForcesPool[PlayingEffect->index-1].EnveLpoePara->EnvelopeReport.fadeTime;
        int32_t elapsedTime = PlayingEffect->Timoffset;
        newValue = magnitude;
        if (attackTime > 0 && elapsedTime < attackTime)
        {
            newValue = (magnitude - attackLevel) * elapsedTime / attackTime;
            newValue += attackLevel;
        }
        else if (fadeTime > 0 && elapsedTime > (duration - fadeTime))
        {
            newValue = (magnitude - fadeLevel) * (duration - elapsedTime);
            newValue /= fadeTime;
            newValue += fadeLevel;
        }
        return newValue * value / magnitude;
    }
	
	return newValue;
}

static float GetPositionInPeriod(Playing_t *PlayingEffect){
    Force_t SquForce = _ramPool.ForcesPool[PlayingEffect->index-1];
    uint8_t phase = SquForce.SpecialPara.Periodic.set_periodic_report.phase;
    int16_t period = SquForce.SpecialPara.Periodic.set_periodic_report.period;
    float phase_shift = (phase / 255.0f) * period;
    float t_total = PlayingEffect->Timoffset + phase_shift;
    float t_mod = fmodf(t_total, (float)period);
    float position = t_mod / (float)period;
    return position;
}
int32_t CalConstantForce(Playing_t *PlayingEffect){ //�Ժ㶨�����в�ֵ
    //return _ramPool.ForcesPool[0].SpecialPara.Constant.set_constant_force_report.magnitude;
    return ApplyEnvelope(PlayingEffect, _ramPool.ForcesPool[PlayingEffect->index-1].SpecialPara.Constant.set_constant_force_report.magnitude,255); 
}

int32_t CalRampForce(Playing_t *PlayingEffect){
    const int8_t startMagnitude = _ramPool.ForcesPool[PlayingEffect->index-1].SpecialPara.Ramp.set_ramp_force_report.start;
    const int16_t endMagnitude = _ramPool.ForcesPool[PlayingEffect->index-1].SpecialPara.Ramp.set_ramp_force_report.end;
    const int16_t duration = _ramPool.ForcesPool[PlayingEffect->index-1].EffectPara.set_effect_report.duration;
    int32_t tempforce = (int32_t)(startMagnitude + PlayingEffect->Timoffset * 1.0 * (endMagnitude - startMagnitude) / duration);
    return ApplyEnvelope(PlayingEffect, tempforce,endMagnitude);
} 
int32_t SquareForce(Playing_t *PlayingEffect){
    Force_t SquForce = _ramPool.ForcesPool[PlayingEffect->index-1];
    int8_t offset = SquForce.SpecialPara.Periodic.set_periodic_report.offset;
    uint8_t magnitude = SquForce.SpecialPara.Periodic.set_periodic_report.magnitude;
    int16_t max = offset + magnitude;
    int16_t min = offset - magnitude;
    float pos = GetPositionInPeriod(PlayingEffect);
    if (pos > 0.5){
        return ApplyEnvelope(PlayingEffect, min,magnitude);
    }else{
        return ApplyEnvelope(PlayingEffect, max,magnitude);
    }
}


int32_t TriangleForce(Playing_t *PlayingEffect) {
    Force_t SquForce = _ramPool.ForcesPool[PlayingEffect->index-1];
    uint8_t magnitude = SquForce.SpecialPara.Periodic.set_periodic_report.magnitude;
    int8_t offset = SquForce.SpecialPara.Periodic.set_periodic_report.offset;
    float position = GetPositionInPeriod(PlayingEffect);
    float triangle = 2.0f * (1.0f - fabsf(2.0f * position - 1.0f)) - 1.0f;
    int16_t wave_value = (int16_t)(triangle * magnitude + offset);

    return ApplyEnvelope(PlayingEffect, wave_value, magnitude);
}
int32_t SawtoothUpForce(Playing_t *PlayingEffect) {
    Force_t SquForce = _ramPool.ForcesPool[PlayingEffect->index-1];
    int8_t offset = SquForce.SpecialPara.Periodic.set_periodic_report.offset;
    uint8_t magnitude = SquForce.SpecialPara.Periodic.set_periodic_report.magnitude;
   
    float position = GetPositionInPeriod(PlayingEffect);
    float triangle = 2.0f * position - 1.0f;
    int16_t wave_value = (int16_t)(triangle * magnitude + offset);
    
    return ApplyEnvelope(PlayingEffect, wave_value, magnitude);
}
int32_t SawtoothDownForce(Playing_t *PlayingEffect) {
    Force_t SquForce = _ramPool.ForcesPool[PlayingEffect->index-1];
    int8_t offset = SquForce.SpecialPara.Periodic.set_periodic_report.offset;
    uint8_t magnitude = SquForce.SpecialPara.Periodic.set_periodic_report.magnitude;
    float position = GetPositionInPeriod(PlayingEffect);
    float triangle = -2.0f * position + 1.0f;
    int16_t wave_value = (int16_t)(triangle * magnitude + offset);   
    return ApplyEnvelope(PlayingEffect, wave_value, magnitude);
}
int32_t CalConditionForce(Playing_t *PlayingEffect,float pos,uint8_t Axis){ 
    uint8_t deadBand; 
	int8_t cpOffset; 
	int8_t positiveCoefficient;
	int8_t negativeCoefficient;
	uint8_t positiveSaturation;
	uint8_t negativeSaturation;
    /*
    FIXME: ������bug Axis�п��ܹ���һ��
    */
    deadBand = _ramPool.ForcesPool[PlayingEffect->index-1].SpecialPara.Condition[Axis].set_condition_struct.dead_band;
    cpOffset = _ramPool.ForcesPool[PlayingEffect->index-1].SpecialPara.Condition[Axis].set_condition_struct.cp_offset;
    cpOffset = (cpOffset == -1) ? 0 : cpOffset;
    positiveCoefficient = _ramPool.ForcesPool[PlayingEffect->index-1].SpecialPara.Condition[Axis].set_condition_struct.positive_coefficient;
    positiveCoefficient = positiveCoefficient == -1 ? 0 : positiveCoefficient;
    negativeCoefficient = _ramPool.ForcesPool[PlayingEffect->index-1].SpecialPara.Condition[Axis].set_condition_struct.negative_coefficient;
    negativeCoefficient = negativeCoefficient == -1 ? 0 : negativeCoefficient;
    positiveSaturation = _ramPool.ForcesPool[PlayingEffect->index-1].SpecialPara.Condition[Axis].set_condition_struct.positive_saturation;
    negativeSaturation = _ramPool.ForcesPool[PlayingEffect->index-1].SpecialPara.Condition[Axis].set_condition_struct.negative_saturation;

    float tempForce = 0;
    if(pos < (cpOffset-deadBand)){
        tempForce = (pos- (float)(cpOffset - deadBand) ) * negativeCoefficient / 127.0f;
		tempForce = (tempForce < -negativeSaturation ? -negativeSaturation : tempForce);
    }
    else if(pos > (cpOffset+deadBand)){
        tempForce = (pos - (float)(cpOffset + deadBand) ) * positiveCoefficient / 127.0f;
		tempForce = (tempForce > positiveSaturation ? positiveSaturation : tempForce);
    }
    else return 0;

    return (int32_t) (tempForce * DeviceGain /255);
} 

int32_t SineForce(Playing_t *PlayingEffect){
    Force_t SquForce = _ramPool.ForcesPool[PlayingEffect->index-1];
    int8_t offset = SquForce.SpecialPara.Periodic.set_periodic_report.offset;
    uint8_t magnitude = SquForce.SpecialPara.Periodic.set_periodic_report.magnitude;

    float rad = GetPositionInPeriod(PlayingEffect)*360.0f*DEG_TO_RAD;
    return ApplyEnvelope(PlayingEffect, (int16_t)(sin(rad)*magnitude + offset),magnitude);
}
int32_t GetEffectForce(Playing_t *Effect,uint8_t Axis){
    int32_t value = 0;
    Force_t force = _ramPool.ForcesPool[Effect->index-1];
    uint8_t direction;
    if(force.EffectPara.set_effect_report.axes_enable_direction & DIRECTION_ENABLE){
        direction = force.EffectPara.set_effect_report.direction_with_ordinals_1;
    }
    else{
        direction = (Axis == 0 ? force.EffectPara.set_effect_report.direction_with_ordinals_1 : force.EffectPara.set_effect_report.direction_with_ordinals_2);
    }
    float angle = (direction * 360.0 / 255.0) * DEG_TO_RAD;
    float angle_ratio = Axis == 0 ? sin(angle) : cos(angle);
    
    uint8_t EffectType = _ramPool.ForcesPool[Effect->index-1].EffectPara.set_effect_report.effect_type;
    switch (EffectType)
    {
        case CONSTANT:
            value = CalConstantForce(Effect)*force.EffectPara.set_effect_report.gain / 255.0f * angle_ratio;
            break;
        case RAMP:
            value = CalRampForce(Effect) * force.EffectPara.set_effect_report.gain / 255.0f * angle_ratio;
            break;
        case SPRING:
            float angle = Axis == 0 ? GetAxisPos(0)*360 : GetAxisPos(1)*360;
            float AngleLimitMin = Axis == 0 ? -g_ParaList.XAngleLimit/2.0f : -g_ParaList.YAngleLimit/2.0f;
            float AngleLimitMax = Axis == 0 ? g_ParaList.XAngleLimit/2.0f : g_ParaList.YAngleLimit/2.0f;    
            value = CalConditionForce(Effect,linear_map_f(angle,AngleLimitMin,AngleLimitMax,-127.0f,127.0f),Axis);
            //printf("value:%ld\n",value);
            break;
        case FRICTION:
            float speed_F = Axis == 0 ? GetAxisSpeed(0)  : GetAxisSpeed(1);
            if (fabs(speed_F) <= 0.2) {

                speed_F = 0.0f;
            } else {

                float direction = speed_F > 0 ? 1.0f : -1.0f;
                float absSpeed = fabs(speed_F);
                

                float normalized = (absSpeed - 0.2) / 0.5;

                speed_F = direction * fminf(normalized, 1.0f) * 127;
            }
            value = CalConditionForce(Effect,speed_F,Axis);
            break;
        case DAMPER:
            float speed_D = Axis == 0 ? GetAxisSpeed(0)  : GetAxisSpeed(1);
            value = CalConditionForce(Effect,linear_map_f(speed_D,-5,5,-127.0f,127.0f),Axis); 
            break;
        case SQUARE:
            value = SquareForce(Effect)* force.EffectPara.set_effect_report.gain / 255.0f * angle_ratio;
            break;
        case SINE:
            value = SineForce(Effect)* force.EffectPara.set_effect_report.gain / 255.0f * angle_ratio;
            break;
        case TRIANGLE:
            value = TriangleForce(Effect)* force.EffectPara.set_effect_report.gain / 255.0f * angle_ratio;
            break;
        case SAWTOOTH_UP:
            value = SawtoothUpForce(Effect)* force.EffectPara.set_effect_report.gain / 255.0f * angle_ratio;
            break;
        case SAWTOOTH_DOWN:
            value = SawtoothDownForce(Effect)* force.EffectPara.set_effect_report.gain / 255.0f * angle_ratio;
            break;
        default:
            break;
    }
    return value;
}
int i = 0;
void Output(PlayingManager *_PlayingManager,QueueHandle_t xQueue){ // tick max 0x7ff
    int32_t value = 0;
    ForceValue_t Force = {0,0};
    for(int i=0; i < MAX_EFFECTS_NUM;i++){
        Playing_t *_effect = &_PlayingManager->PlayList[i];
        
        const int16_t duration = _ramPool.ForcesPool[_effect->index-1].EffectPara.set_effect_report.duration;

        if(_effect->PlayState != PLAYING){ 
            continue;
        }else{
            if(duration == INFINITE_DURATION || (_effect->Timoffset) <= duration ){
                
                Force.X += GetEffectForce(_effect,0);
            }
            else{
                _effect->Playnum -- ; 
                _effect->Timoffset = 0; 
                //value[0] = GetEffectForce(&_PlayingManager->PlayList[i],0);
                //value[1] = GetEffectForce(&_PlayingManager->PlayList[i],1);
            } 
            if (_effect->Playnum <= 0)
            {
                _effect->PlayState = STOPPED; //ͣ
            }   
        }   
    }

    xQueueSend(xQueue,&value,0); 
}
static IRAM_ATTR bool timer_isr(void *para){
    PlayingManagerUpdate(&EffectPlayingManager);
    timer_group_clr_intr_status_in_isr(TIMER_GROUP_0, TIMER_0);
    return true;
}
static void HardWareTimerInit(void){
    timer_config_t config = {
        .divider = 80,
        .counter_dir = TIMER_COUNT_UP,
        .auto_reload = true,
        .alarm_en = TIMER_ALARM_EN,
    };
    timer_init(TIMER_GROUP_0, TIMER_0, &config);
    timer_set_counter_value(TIMER_GROUP_0, TIMER_0, 0);
    timer_set_alarm_value(TIMER_GROUP_0, TIMER_0, 1000); // 1ms

    //timer_isr_callback_add(TIMER_GROUP_0, TIMER_0, , NULL, 0);
    timer_isr_callback_add(TIMER_GROUP_0, TIMER_0, timer_isr, NULL, ESP_INTR_FLAG_IRAM);

    timer_start(TIMER_GROUP_0, TIMER_0);
}
void InitFFB(void){
    InitRamPool();
    PlayingManagerInit(&EffectPlayingManager);
    INIT_ALL_STATE(worldModel.deviceState);
    HardWareTimerInit();
}
//FIXME:修改了全局参数列表，这个函数要大改
// float ApplyWheelAngleLimit(float angle,float value){
//     int16_t _limitMin = - GlobalConfig.XAngleLimit / 2;
//     int16_t _limitMax = GlobalConfig.XAngleLimit / 2;
//     if(angle < _limitMin){
//         float overLimit = angle - _limitMin;
//         float torque = overLimit * GlobalConfig.AngleLimitGain;
//         torque = (torque<-GlobalConfig.AngleLimitMaxTorque)? -GlobalConfig.AngleLimitMaxTorque:torque;
//         return torque;
//     }
//     if(angle > _limitMax){
//         float overLimit = angle - _limitMax;
//         float torque = overLimit * GlobalConfig.AngleLimitGain;
//         torque = (torque>GlobalConfig.AngleLimitMaxTorque)? GlobalConfig.AngleLimitMaxTorque:torque;
//         return torque;
//     }
//     return value;
    
// }
